#include "SypEngine.h"

#ifdef CHECKMEM
#include "mmgr.h"
#endif
namespace Syp
{

const Point4 Node::getPosition()
{	
	return pos;
}

void Node::bindToNode(Node* n)
{
	if(n)
	{
		if(parent)
		{
			this->unBindFromParent();
		}
		parent = n;
		n->children.push_back(this);
		//std::cout<<"Added child "<<this<<"\n";
//		std::cout<<"Parent "<<n<<" has "<<n->children.size()<<"\n";
	}

}

Node::Node():parent(0),nodeDying(0)
{
	Math::IdentityMatrix(mat);
}

Node::Node(const Node& node)
{
	parent = 0;
	nodeDying = 0;
	clone(node);
}

Node& Node::operator =(const Node& node)
{
	clone(node);
	return *this;
}

void Node::clone(const Node& node)
{
	bindToNode(node.parent);
	for( int i=0; i < 16; ++i )
	{
		mat[i] = node.mat[i];
	}
}


void Node::unBindFromParent()
{
	if(parent)
	{
		//Unlink from parent, remove self from parent's children list
		//	std::cout<<"Removing child "<<this<<"\n";
		if(!parent->nodeDying)
		{//if the parent node is not dying, we need to remove the references from the parent's child registry.
	//		std::cout<<"Attempting Parent "<<parent<<" removed child "<<this<<" and has "<<parent->children.size()<<"\n";
			std::vector<Node*>::iterator it = std::find(parent->children.begin(),parent->children.end(),this);
			if(it != parent->children.end())
			{
				parent->children.erase(it);
			}
			else
			{
				assert(0 && "Something is wrong somwhere");
			}
//			std::cout<<"Parent "<<parent<<" removed child "<<this<<" and has "<<parent->children.size()<<"\n";
		}

		parent = 0;
	}
}

Node::~Node()
{
	nodeDying = true;
	unBindFromParent();

	//Children matters, bid farewell to them, we do it inside so as to prevent iterator
	std::vector<Node*>::iterator a = children.begin();
	std::vector<Node*>::iterator b = children.end();

	while(a!=b)
	{
		(*a)->unBindFromParent();//ok gothca, we now have no parent
		++a;
	}
}
void Node::rotateAroundLocalAxis(AngleAxis a)
{
	/*
	float matrix[16];
	float ans[16];
	
	Math::getMatrix(a, matrix);
	
	Math::multMat(matrix,mat, ans);
	setMatrix(ans);
	*/

	a = a.convertToQuat();
	rot = a * rot;

	//angle *= (PI/180.0f);//convert to radian
	//axis %= 3;
 //   int j = (axis + 1) % 3;
 //   int k = (j + 1) % 3;
	//
 //   float s, c;
	//s = sin(angle * 0.5f);
	//c = cos(angle * 0.5f);
 //  
 //   float q[4], m_[4];
	//m_[0] = rot.x;
	//m_[1] = rot.y;
	//m_[2] = rot.z;
	//m_[3] = rot.w;

 //   q[axis] = c * m_[axis] + s * m_[3];
 //   q[j] = c * m_[j] - s * m_[k];
 //   q[k] = c * m_[k] + s * m_[j];
 //   q[3] = c * m_[3] - s * m_[axis];

 //  rot.x = q[0];
 //  rot.y = q[1];
 //  rot.z = q[2];
 //  rot.w = q[3];
 //  rot.Normalize();

}

void Node::setMatrix(float* matrix)
{
	for( int i = 0; i < 16; ++i )
	{
		mat[i] = matrix[i];
	}
}

float* Node::getMatrix()
{
	return mat;
}

void Node::rotateAroundWorldAxis(AngleAxis a)
{
	/*
	float rot[16];
	float ans[16];	
	float inv[16];
	float axis[3];
	float axis2[3];

	axis[0] = a.x;
	axis[1] = a.y;
	axis[2] = a.z;
		
	//Math::getMatrix(a, matrix);//rotation matrix

	Math::InverseMatrix(mat, inv);//find inverse of it

	Math::MultNormalByMatrix(inv, axis,1.0f, axis2);

	a.x = axis2[0];
	a.y = axis2[1];
	a.z = axis2[2];
	Math::getMatrix(a, rot);//get our new rotation matrix
	Math::multMat(rot, mat, ans);
	setMatrix(ans);
	*/

	a = a.convertToQuat();
	rot = rot * a;
	//rot.Normalize();

	//angle *= (PI/180.0f);//convert to radian
	//axis %= 3;
 //   int j = (axis + 1) % 3;
 //   int k = (j + 1) % 3;
	//
 //   float s, c;
	//s = sin(angle * 0.5f);
	//c = cos(angle * 0.5f);
 //  
 //   float q[4], m_[4];
	//m_[0] = rot.x;
	//m_[1] = rot.y;
	//m_[2] = rot.z;
	//m_[3] = rot.w;
 //   q[axis] = c * m_[axis] + s * m_[3];
 //   q[j] = c * m_[j] + s * m_[k];
 //   q[k] = c * m_[k] - s * m_[j];
 //   q[3] = c * m_[3] - s * m_[axis];
 //  rot.x = q[0];
 //  rot.y = q[1];
 //  rot.z = q[2];
 //  rot.w = q[3];
 //  rot.Normalize();

}
void Node::rotateBy(Quat& q)
{
	setOrientation(q * getOrientation());
}

const Quat Node::getOrientation()
{	
	/*float T =  mat[0] + mat[5] + mat[10] + 1;
	Quat q;
	float s = 0;
	if( T > 0 )
	{
		s = 0.5f / sqrt(T);
		q.w = 0.25f / s;
		q.x = (mat[9] - mat[6]) * s;
		q.y = (mat[2] - mat[8]) * s;
		q.z = (mat[4] - mat[1]) * s;
	}
	else
	{
		if( mat[0] > mat[5] && mat[0] > mat[10] )
		{
			s  = sqrt( 1.0f + mat[0] - mat[5] - mat[10] ) * 2;

			q.x = 0.5f / s;
			q.y = (mat[1] + mat[4] ) / s;
			q.z = (mat[2] + mat[8] ) / s;
			q.w = (mat[6] + mat[9] ) / s;
		}
		else if( mat[5] > mat[0] && mat[5] > mat[10] )
		{
			s  = sqrt( 1.0f + mat[5] - mat[0] - mat[10] ) * 2;

			q.x = (mat[1] + mat[4] ) / s;
			q.y = 0.5f / s;
			q.z = (mat[6] + mat[9] ) / s;
			q.w = (mat[2] + mat[8] ) / s;

		}
		else
		{
			s  = sqrt( 1.0f + mat[10] - mat[0] - mat[5] ) * 2;

			q.x = (mat[2] + mat[8] ) / s;
			q.y = (mat[6] + mat[9] ) / s;
			q.z = 0.5f / s;
			q.w = (mat[1] + mat[4] ) / s;

		}
	}
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;


	return q;*/

	return rot;
	
}

Node* Node::getParentPtr()
{
	return parent;
}

void Node::setPosition(float x, float y, float z,float w)
{
	if(children.size() > 0)
	{
		Point3 offset;
		offset.x = x - pos.x;
		offset.y = y - pos.y;
		offset.z = z - pos.z;
		std::vector<Node*>::iterator a = children.begin();
		std::vector<Node*>::iterator b = children.end();
		while(a != b)
		{
			(*a)->offsetPosition(offset.x,offset.y,offset.z);
			++a;
		}
	}
	
	pos.x = x;
	pos.y = y;
	pos.z = z;
	pos.w = w;
	

}

void Node::setOrientation(float x, float y, float z,float w)
{
	/*Quat rot;
	rot.x = x;
	rot.y = y;
	rot.z = z;
	rot.w = w;
	float ox,oy,oz;
	ox = mat[12];
	oy = mat[13];
	oz = mat[14];
	Math::QuatToMatrix(rot, mat);
	mat[12] = ox;
	mat[13] = oy;
	mat[14] = oz;	*/
	rot.x = x;
	rot.y = y;
	rot.z = z;
	rot.w = w;
}

void Node::setOrientation(Quat& q)
{
	setOrientation(q.x, q.y, q.z, q.w);
}
void Node::offsetPosition(float x, float y, float z)
{
	if(children.size() > 0)
	{	
		std::vector<Node*>::iterator a = children.begin();
		std::vector<Node*>::iterator b = children.end();
		while(a != b)
		{
			(*a)->offsetPosition(x, y, z);
			++a;
		}
	}
	pos.x += x;
	pos.y += y;
	pos.z += z;	
}
}
